#include "path_generate/p2p/astar.h"
#include <queue>
#include <unordered_map>
#include <vector>
#include <algorithm>  // for std::reverse

struct CompareNodes {
    bool operator()(const GridNodePtr& lhs, const GridNodePtr& rhs) const {
        return lhs->getF() > rhs->getF() || (lhs->getF() == rhs->getF() && lhs->getH() > rhs->getH());
    //     return lhs->getF() > rhs->getF() || 
    //         ((lhs->getF() >= rhs->getF() - 0.000001) 
    //             && (lhs->getF() <= rhs->getF() + 0.000001)
    //             && lhs->getH() > rhs->getH());
    }
};

AstarPlanner::AstarPlanner(){
    debugMapPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/grid_expend_map", 1, true);
}

// 生成路径
void AstarPlanner::genGridPath(int sx, int sy, int gx, int gy, vector<pair<int, int>>& points){
    LOG(INFO) <<  "start: (" << sx << "," << sy
        << "), goal: (" << gx << "," << gy << ")";
    
    if(debug_){ //调试模式
        expend_points_.clear();
        debugGrid_ = getOccupancyGrid();
    }
    int width = getWidth();

    // 设置起点、终点
    GridNode nodeStart(sx, sy, 0, 0);
    nodeStart.setIdx(width);
    
    GridNode nodeGoal(gx, gy, 0, 0);
    nodeGoal.setIdx(width);

    // 初始化列表
    nodeStart.updateH(nodeGoal);
    std::priority_queue<GridNodePtr, std::vector<GridNodePtr>, CompareNodes> openList;
    std::unordered_map<int, float> closedList;  //存储G值
    openList.push(std::make_shared<GridNode>(nodeStart));   

    int iteration = 0;
    while(!ros::isShuttingDown() && !openList.empty()){
        iteration++;
        // if(debug_){ //调试模式
        //     cout << "iteration: " << iteration << endl;
        // }
        
        // 获取优先级最高节点
        auto nodePred = openList.top();
        openList.pop();
        int idxPred = nodePred->getIdx();


        if(closedList.find(idxPred) != closedList.end()){
            continue;
        }
        closedList[idxPred] = nodePred->getG();

        if(debug_){ //调试模式
            expend_points_.insert(idxPred);
            // cout << "idxPred: " << idxPred 
            //     << ", x: " << nodePred->getX() 
            //     << ", y: " << nodePred->getY()
            //     << ", cost: " << nodePred->getF()
            //     << endl;
        }

        // 发现目标节点
        if(*nodePred == nodeGoal){
            LOG(INFO) << "find goal. iteration: " << iteration;
            while(nodePred->getPred() != nullptr){
                points.emplace_back(make_pair(nodePred->getX(), nodePred->getY()));
                nodePred = nodePred->getPred();
            }  
            std::reverse(points.begin(), points.end());   
            LOG(INFO) << "points size: " << points.size();
            break;
        }

        // 遍历邻域节点
        for(int i = 0; i < GridNode::dir; i++) {
            int newX = nodePred->getX() + GridNode::dx[i];
            int newY = nodePred->getY() + GridNode::dy[i];
            auto nodeSucc = std::make_shared<GridNode>(newX, newY, nodePred->getG(), 0); 
            nodeSucc->setPred(nodePred);
            int idxSucc = nodeSucc->setIdx(width);

            // 障碍物检测 是否在闭列表
            if(!valueIsValidByIdx(idxSucc)
                || closedList.find(idxSucc) != closedList.end()){
                continue;
            }   

            // G值比较
            nodeSucc->updateG();
            float newG = nodeSucc->getG();
            if(closedList.find(idxSucc) == closedList.end() || newG < closedList[idxSucc]) {
                nodeSucc->updateH(nodeGoal);
                // LOG(INFO) << "--> i: " << i 
                //     << ", idxSucc: " << idxSucc 
                //     << ", x: " << nodeSucc->getX() 
                //     << ", y: " << nodeSucc->getY()
                //     << ", g: " << nodeSucc->getG()
                //     << ", h: " << nodeSucc->getH()
                //     << ", cost: " << nodeSucc->getF();
                openList.push(nodeSucc);
            } 
        }
    }

    if(debug_){ //调试模式
        LOG(INFO) << "openList size: " << openList.size() 
            << ", closedList size: " << closedList.size();
        for(auto idx : expend_points_){
            if(idx >=0 && idx < debugGrid_.data.size()){
                debugGrid_.data[idx] = 30;
            }
        }
        debugMapPub_.publish(debugGrid_);
    }
}